#ifndef SINGLE_INNFOS_JOINT_H
#define SINGLE_INNFOS_JOINT_H

#include <iostream>
#include "actuatorcontroller.h"
#include <thread>
#include <signal.h>
#include <string.h>
#include <chrono>
#include <math.h>
#include <Eigen/Eigen>
#include <vector>
#include <atomic>
//#include other .h 

using namespace std;

class single_innfos_joint
{
public:	
	//构造和析构函数	
	single_innfos_joint(const string ifconfig, const int ip, const int joint_id);
	/**
     * @param ifconfig 网卡名称，可通过ifconfig查询, 如eno1，则ifconfig = "eno1"
     * @param ip ip地址，如ip=1，则实际ip为192.168.1.1
     * @param joint_id 关节id
    **/
	~single_innfos_joint(); 
	
	//is joint on? If false, on() will print error information, and other public function will do nothing
	bool is_on = false;

	void on(bool zero_or_not = false);
	/**
     * @brief 使能关节。如果之前没有找到关节，会自动再找一次
     * @param zero_or_not  是否需要归零，默认不需要
    **/
    
    void zero();
    /**
     * @brief 设置关节零位
    **/
    
	void off();
	/**
     * @brief 失能关节
    **/
	
	void poscontrol_on(double vellimit = 1200, double accellimit = 4800);
	/**
     * @brief 将关节设置为位置控制模式
     * @param vellimit  最大速度，默认值1200
     * @param accellimit 最大加速度，默认值4800
    **/
    
    void velcontrol_on(double accellimit = 1200);
	/**
     * @brief 将关节设置为速度控制模式
     * @param accellimit 最大加速度，默认值1200
    **/
    
    void curcontrol_on();
	/**
     * @brief 将关节设置为电流控制模式
    **/
    
    void to_absolute_position(double ab_pos, bool check_not = true);
    /**
     * @brief 在位置模式下将关节运动到一个绝对角度
     * @param ab_pos 绝对角度值，单位rad
     * @param check_not 是否检查运动完成，默认true
    **/
    
    void to_relative_position(double re_pos, bool check_not = true);
    /**
     * @brief 在位置模式下将关节从一个初始位置运动一个相对角度
     * @param re_pos 相对角度值，单位rad
     * @param check_not 是否检查运动完成，默认true
    **/
    
    void rotate(double target_vel);
    /**
     * @brief 在速度模式下使关节按照一定速度转动，转动轴由输出轴指向端盖，正方向满足右手定则。使用这条指令需要使用rotate_stop来使关节停止转动
     * @param target_vel 目标速度，非零，单位
    **/
    
    void rotate_time(double target_vel, int ms);
    /**
     * @brief 在速度模式下使关节按照一定速度转动，ms毫秒后停止转动
     * @param target_vel 目标速度，非零，单位
     * @param ms 转动时间，单位ms
    **/
    
    void rotate_cur_limit(double target_vel, double cur_limit);
    /**
     * @brief 在速度模式下使关节按照一定速度转动，直到遇到碰撞（即电流超过电流限制）
     * @param target_vel 目标速度，非零，单位
     * @param cur_limit 电流限制，没有正负，当电流超过该值后关节停止运动。
    **/
    
    void rotate_stop();
    /**
     * @brief 使关节停止转动
    **/
    
    void data_feedback(double ** pos_fb, double ** vel_fb, double ** cur_fb);
    /**
     * @brief 位置、速度、电流三个数据的读取，主程序中调用时应为data_feedback(&pos_fb,&vel_fb,&cur_fb),具体说明如下
     * @param pos_fb double指针，运行完该程序后将指向关节的位置数据，主程序中pos_fb定义为double * pos_fb，主程序中调用该函数的参数形式为&pos_fb，主程序调用关节位置时使用*pos_fb
     * @param vel_fb double指针，运行完该程序后将指向关节的速度数据，主程序中pos_fb定义为double * vel_fb，主程序中调用该函数的参数形式为&vel_fb，调用关节速度时使用*vel_fb
     * @param cur_fb double指针，运行完该程序后将指向关节的电流数据，主程序中pos_fb定义为double * cur_fb，主程序中调用该函数的参数形式为&cur_fb，调用关节电流时使用*cur_fb
    **/

protected:
	//protected members here;
	//normally, most of variables and inside functions are here;

	//joint's position, velocity, current data (real time)
	double * position;		//单位：rad
	double * velocity;		//单位：
	double * current;		//单位：A

	//string of ifconfig order
	char * str2char;
	
	//class point
	ActuatorController * pController;
	
	//ec Define an error type, ec==0x00 means no error, ec will be passed to pcontroller-> lookupActuators(ec) by reference,
    //when the error occurs, ec value will be modified by SDK to the corresponding error code
    Actuator::ErrorsDefine ec;
	
	//actuator array for all joints
	vector<ActuatorController::UnifiedID> uIDArray;
	
	//actuator finding times
	int finding_attempt;
	
	//thread for data request
	pthread_t pth;
	
	//rotation has stopped due to current limit or not
	bool rotate_stop_not;
	
	//

private:
	//private members here;
};

#endif
